#include "stdlib.h"
#include "string.h"
#include  "task_mix.h"
#include "task_system.h"

#include "FreeRTOS.h"
#include "task.h"
#include "adc_driver.h"
#include "switch_driver.h"
#include "trim_driver.h"
#include "trainer_driver.h"
#include "led_driver.h"

#include "stdlib.h"
#include "stdio.h"

#include "platform.h"
#include "mathematics.h"


Driver *p_rf_driver;
short analog_raw[NUM_POTS];
short analog_calibrated[NUM_POTS];
short analog_filtered[NUM_POTS];
short analog_pot[NUM_POTS];

short trainer_raw[4];
short trainer_calibrated[4];

char switch_sw[NUM_SW_POSITIONS];
char trim_raw[8];

short analog_vpot[NUM_VPOT];
short switch_vsw[NUM_VSW];

#define RESOLUTION (4096)
#define HALF_RESOLUTION (RESOLUTION/2)

#define FSM_CALIBRATION_NULL (0)
#define FSM_CALIBRATION_START (1)
#define FSM_CALIBRATION_RUN (2)
#define FSM_CALIBRATION_STOP (3)
#define FSM_CALIBRATION_DONE (4)

static char calibration_fsm=FSM_CALIBRATION_NULL;
void startCalibration()
{
    calibration_fsm =FSM_CALIBRATION_START;
}

void stopCalibration()
{
    calibration_fsm =FSM_CALIBRATION_STOP;
}


void setDefaultTransmiterFile()
{
    int i;

    memset(&transmiterFile, 0, sizeof(transmiterFile));
    /// version
    transmiterFile.version =0x10;

    /// cali
    for(i=0; i<NUM_POTS; i++)
    {
        transmiterFile.cali[i].up=3071;
        transmiterFile.cali[i].low=1024;
        transmiterFile.cali[i].mid=2000;
        transmiterFile.cali[i].filter=0;
        transmiterFile.cali[i].invert =0;
    }

    /// 
    transmiterFile.currModel =0;
    /// stick mode
    transmiterFile.stickMode =0;
    /// virtual source
    for(i=0; i<NUM_VSW; i++)
    {
        transmiterFile.vSW[i].source.type =SRC_TYPE_VSW;
        transmiterFile.vSW[i].source.id =0;    /// const_0
    }

    for(i=0; i<NUM_POTS; i++)
    {
        transmiterFile.vPot[i].source.type =SRC_TYPE_VSW;
        transmiterFile.vPot[i].source.id =0; ///< false
        transmiterFile.vPot[i].alg.bool_pot.value =0; ///< center
    }

};

void setDefaultModelFile()
{
};

const unsigned char RETA_map[4][4] ={
    {0,1,2,3},
    {0,2,1,3},
    {3,1,2,0},
    {3,2,1,0} };
    
int getStickThr(uint8_t stick_mode) {    return analog_calibrated[RETA_map[stick_mode][2]];}
int getStickAil(uint8_t stick_mode) {    return analog_calibrated[RETA_map[stick_mode][3]];}
int getStickEle(uint8_t stick_mode) {    return analog_calibrated[RETA_map[stick_mode][1]];}
int getStickRud(uint8_t stick_mode) {    return analog_calibrated[RETA_map[stick_mode][0]];}


void debug_int(int *analog_raw, int min, int max, int len)
{
    int i;
        printf("\\\\\\\\\\\\\n");
        {
            int t_write[NUM_POTS];

            //write scope
            t_write[0] = min;
            t_write[1] =max;

            fwrite(t_write, 4, 2, stdout);

            //write values
            for(i=0; i<len; i++)
            {
                t_write[i] =analog_raw[i];
            }
            fwrite(t_write, 1, sizeof(t_write),stdout);
        }
        printf("\\\\\\\\\\\\");

}

void debug_short(short *analog_raw, int min, int max, int len)
{
    int i;
        printf("\\\\\\\\\\\\\n");
        {
            int t_write[64];

            //write scope
            t_write[0] = min;
            t_write[1] =max;

            fwrite(t_write, 4, 2, stdout);

            //write values
            for(i=0; i<len; i++)
            {
                t_write[i] =analog_raw[i];
            }
            fwrite(t_write, 1, len*sizeof(t_write[0]),stdout);
        }
        printf("\\\\\\\\\\\\");

}

/// calibrate pots, by transmiterFile->cali
void calibrate(short *output, short *input, int len)
{
    int i;
    int t_int;

    CaliData *calidata;
    for(i=0; i<len;i++)
    {
        t_int =*input++;
        calidata =&transmiterFile.cali[i];

        if(t_int > calidata->mid)
        {
            t_int =(t_int-calidata->mid)*(HALF_RESOLUTION) /(calidata->up-calidata->mid) ;
            t_int =t_int>(HALF_RESOLUTION-1)? (HALF_RESOLUTION-1): t_int;
        }
        else
        {
            t_int =(t_int-calidata->mid)*HALF_RESOLUTION/(calidata->mid-calidata->low);
            t_int =(t_int<-HALF_RESOLUTION)? -HALF_RESOLUTION:t_int;
        }
        
        if(calidata->invert)
        {
            *output++ =2*HALF_RESOLUTION -t_int;
        }
        else
        {
            *output++ =t_int;
        }
    }
}

void calibrate_trainer(short *output, short *input)
{
    int i;
    int t_int;

    for(i=0; i<4; i++)
    {
    }
}

/// apply input filter, by transmiterFile->cali
void input_filter(short *output, short *input, int len)
{
    int i;
    int t_int;
    for( i=0; i<len; i++)
    {
        //assert(transmiterFile.filter<12);
        transmiterFile.cali[i].filter =transmiterFile.cali[i].filter>12? 12: transmiterFile.cali[i].filter;
        
        t_int =1<<(12-transmiterFile.cali[i].filter);
        if(*output>*input)
        {
            if( (*output - t_int)<*input)
            {
                *output =*input;
            }else
            {
                *output -=t_int;
            }
        }
        else
        {
            if(*output+t_int >*input)
            {
                *output =*input;
            }else
            {
                *output +=t_int;
            }
        }
        input++;
        output++;
    }
}

/**
 * @fn get_vsw_ch
 * @brief create a virtual switch result: true or false
 *          sources: POT/VSW(can not be self)/SW/VPOT
 *          POT: 1: when pot_value> pot_bool_alg.value+bandgap
 *          VPOT: 1: when vpot_value >pot_bool_alg.value+bandgap
 *          SW: result produced by source_sw and bool_bool_alg.ch2 and method
 *          VSW: the same criteria as above line
 *
 */
 #define VCH_BAND_GAP (20)

int get_vsw_ch(int channel)
{
    int ret_val;
    VChannel& VCh =transmiterFile.vSW[channel];

    ret_val = switch_vsw[channel];
    /// source: vpot
    if(VCh.source.type == SRC_TYPE_VPOT)
    {
        if(analog_vpot[VCh.source.id] > VCh.alg.pot_bool.value+VCH_BAND_GAP) ret_val=1;
        else if(analog_vpot[VCh.source.id] < VCh.alg.pot_bool.value -VCH_BAND_GAP) ret_val =0;
        else ret_val =switch_vsw[channel];
    }

    /// CONST
    else if(VCh.source.type == SRC_TYPE_CONST)
    {
        ret_val =VCh.alg.const_value.bool_value;
    }

    /// source: vsw / vsw(sw)
    else if(VCh.source.type ==SRC_TYPE_VSW)
    {
        /// self? disabled
        if(VCh.source.id ==channel) return switch_vsw[channel];

            
        switch(VCh.alg.bool_bool.boolean)
        {
            /// !src
            case BOOLEAN_NOT:
                ret_val = ! switch_vsw[VCh.source.id];
                break;
            /// src || ch2
            case BOOLEAN_OR:
                if(VCh.alg.bool_bool.ch2.type ==SRC_TYPE_VSW)
                {
                    /// self logic disabled
                    if(VCh.alg.bool_bool.ch2.id == channel) return switch_vsw[channel];
                    ret_val  =switch_vsw[VCh.source.id] || switch_vsw[VCh.alg.bool_bool.ch2.id];
                }
                else if(VCh.alg.bool_bool.ch2.type == SRC_TYPE_SW)
                {
                    ret_val =switch_vsw[VCh.source.id] || switch_sw[VCh.alg.bool_bool.ch2.id];
                }
                else
                {
                    ret_val =switch_vsw[VCh.source.id];
                }
                break;
            /// src && ch2
            case BOOLEAN_AND:
                if(VCh.alg.bool_bool.ch2.type ==SRC_TYPE_VSW)
                {
                    /// self logic disabled
                    if(VCh.alg.bool_bool.ch2.id == channel) return switch_vsw[channel];
                    ret_val  =switch_vsw[VCh.source.id] && switch_vsw[VCh.alg.bool_bool.ch2.id];
                }
                else if(VCh.alg.bool_bool.ch2.type == SRC_TYPE_SW)
                {
                    ret_val =switch_vsw[VCh.source.id] && switch_sw[VCh.alg.bool_bool.ch2.id];
                }
                else
                {
                    ret_val =switch_vsw[VCh.source.id];
                }
                break;
        }
    }

    /// source sw /vsw(sw)
    else if(VCh.source.type ==SRC_TYPE_SW)
    {
        switch(VCh.alg.bool_bool.boolean)
        {
            /// !src
            case BOOLEAN_NOT:
                ret_val = ! switch_sw[VCh.source.id];
                break;
            /// src || ch2
            case BOOLEAN_OR:
                if(VCh.alg.bool_bool.ch2.type ==SRC_TYPE_VSW)
                {
                     if(VCh.alg.bool_bool.ch2.id == channel) return switch_vsw[channel];
                   
                    ret_val =switch_sw[VCh.source.id] || switch_vsw[VCh.alg.bool_bool.ch2.id];
                }
                else if(VCh.alg.bool_bool.ch2.type == SRC_TYPE_SW)
                {
                    ret_val =switch_sw[VCh.source.id] || switch_sw[VCh.alg.bool_bool.ch2.id];
                }
                else
                {
                    ret_val =switch_sw[VCh.source.id];
                }
                break;
            /// src && ch2
            case BOOLEAN_AND:
                if(VCh.alg.bool_bool.ch2.type ==SRC_TYPE_VSW)
                {
                     if(VCh.alg.bool_bool.ch2.id == channel) return switch_vsw[channel];
                   
                    ret_val =switch_sw[VCh.source.id] && switch_vsw[VCh.alg.bool_bool.ch2.id];
                }
                else if(VCh.alg.bool_bool.ch2.type == SRC_TYPE_SW)
                {
                    ret_val =switch_sw[VCh.source.id] && switch_sw[VCh.alg.bool_bool.ch2.id];
                }
                else
                {
                    ret_val =switch_sw[VCh.source.id];
                }
                break;
        }
    }

    /// source : pot
    else if(VCh.source.type ==SRC_TYPE_POT)
    {
        if(analog_pot[VCh.source.id] > VCh.alg.pot_bool.value+VCH_BAND_GAP) ret_val=1;
        else if(analog_pot[VCh.source.id] < VCh.alg.pot_bool.value-VCH_BAND_GAP) ret_val=0;
        else ret_val =switch_vsw[channel];
    }

    return ret_val;
}

 
void get_virtual_switches()
{
    int i;

    for(i =0; i<NUM_VSW; i++)
    {
        switch_vsw[i] =get_vsw_ch(i);
    }
}


/**
 * @fn get_vpot_ch
 * @brief create Vpot values
 *       Sources are SW/VSW/POT/VPOT
 *       POT/VPOT: calue produced by a 7 point line
 *       SW/VSW:  when SW is TRUE: value is bool_pot_alg.value;
 *                       when SW is faluse: value depend on bool_pot_alg.ch2
 *         for ch2, must be POT/VPOT
 *         
 * produce virtual pots, by transmiterFile->vPot
 */
short get_vpot_ch(int channel)
{
    short ret_val;
    
    VChannel &VCh =transmiterFile.vPot[channel];
    ret_val =0;

    /// Vpot=>Vpot
    if(VCh.source.type == SRC_TYPE_VPOT)
    {
        /// self? disabled
        if(VCh.source.id ==channel) return analog_vpot[channel];

        if(VCh.alg.pot_pot.type == CURVE_TYPE_SMOOTH)
        {
            ret_val = curve_7pts(analog_vpot[VCh.source.id], VCh.alg.pot_pot.point);
        }
        else if(VCh.alg.pot_pot.type ==CURVE_TYPE_LINE)
        {
            ret_val = line_7pts(analog_vpot[VCh.source.id], VCh.alg.pot_pot.point);
        }
    }

    /// CONST
    else if(VCh.source.type == SRC_TYPE_CONST)
    {
        ret_val =VCh.alg.const_value.pot_value;
    }

    /// VSW => VPOT
    else if(VCh.source.type ==SRC_TYPE_VSW)
    {
        if(switch_vsw[VCh.source.id])
        {
            ret_val =VCh.alg.bool_pot.value;
        }
        else
        {
            if(VCh.alg.bool_pot.ch2.type ==SRC_TYPE_POT)
            {
                ret_val =analog_pot[VCh.alg.bool_pot.ch2.id];
            }
            else if(VCh.alg.bool_pot.ch2.type ==SRC_TYPE_VPOT)
            {
                if(VCh.alg.bool_pot.ch2.id ==channel) return analog_vpot[channel];
                ret_val =get_vpot_ch(VCh.alg.bool_pot.ch2.id);
            }
            else
            {
                ret_val =0;
            }
        }
    }

    /// SW=> VPOT
    else if(VCh.source.type ==SRC_TYPE_SW)
    {
        if(switch_sw[VCh.source.id])
        {
            ret_val =VCh.alg.bool_pot.value;
        }
        else
        {
            if(VCh.alg.bool_pot.ch2.type ==SRC_TYPE_POT)
            {
                ret_val =analog_pot[VCh.alg.bool_pot.ch2.id];
            }
            else if(VCh.alg.bool_pot.ch2.type ==SRC_TYPE_VPOT)
            {
                if(VCh.alg.bool_pot.ch2.id ==channel) return analog_vpot[channel];
                ret_val =get_vpot_ch(VCh.alg.bool_pot.ch2.id);
            }
            else
            {
                ret_val =0;
            }
        }
    }

    /// POT =>VPOT
    else if(VCh.source.type ==SRC_TYPE_POT)
    {
        if(VCh.alg.pot_pot.type == CURVE_TYPE_SMOOTH)
        {
            ret_val = curve_7pts(analog_pot[VCh.source.id], VCh.alg.pot_pot.point);
        }
        else if(VCh.alg.pot_pot.type ==CURVE_TYPE_LINE)
        {
            ret_val = line_7pts(analog_pot[VCh.source.id], VCh.alg.pot_pot.point);
        }
    }

    return ret_val;
}

void get_virtual_pots()
{
    int i;

    for(i=0; i<NUM_VPOT; i++)
    {
        analog_vpot[i] =get_vpot_ch(i);
    }
}

/**
 * @brief set vsw setting from switch
 * @param channel -channel number: 0-32
 *              alg -0:NOT; 1:OR; 2:AND
 *              type -second channel type: 0:POT; 1:SW ; 2:VPOT; 3: VSW; 4: CONST
 *              src2 - second channel id
 *
 */
void set_vsw_sw(int channel, int src, int alg, int type, int src2)
{
    transmiterFile.vSW[channel].source.id=src;
    transmiterFile.vSW[channel].source.type =SRC_TYPE_SW;
    transmiterFile.vSW[channel].alg.bool_bool.boolean =alg;
    transmiterFile.vSW[channel].alg.bool_bool.ch2.id =src2;
    transmiterFile.vSW[channel].alg.bool_bool.ch2.type =type;
}
void set_vsw_vsw(int channel, int src, int alg, int type, int src2)
{
    transmiterFile.vSW[channel].source.id=src;
    transmiterFile.vSW[channel].source.type =SRC_TYPE_VSW;
    transmiterFile.vSW[channel].alg.bool_bool.boolean =alg;
    transmiterFile.vSW[channel].alg.bool_bool.ch2.id =src2;
    transmiterFile.vSW[channel].alg.bool_bool.ch2.type =type;
}

void set_vsw_pot(int channel, int src, int value)
{
    transmiterFile.vSW[channel].source.id =src;
    transmiterFile.vSW[channel].source.type = SRC_TYPE_POT;
    transmiterFile.vSW[channel].alg.pot_bool.value =value;
}
void set_vsw_vpot(int channel, int src, int value)
{
    transmiterFile.vSW[channel].source.id =src;
    transmiterFile.vSW[channel].source.type = SRC_TYPE_VPOT;
    transmiterFile.vSW[channel].alg.pot_bool.value =value;
}

/**
 * @brief set vpot setting from switch
 *      when switch is TRUE, vpot value;
 *      when switch is FALSE, value depends on Channel 2
 * @param channel - 0~31
 * @param src - switch id
 * @param value - value, when src is TRUE
 * @param type2 - type2, type of channel 2
 * @param src2 -id of channel2
 */
 void set_vpot_SW(int channel, int src, int value, int type2, int src2)
{
    transmiterFile.vPot[channel].source.id = src;
    transmiterFile.vPot[channel].source.type =SRC_TYPE_SW;
    transmiterFile.vPot[channel].alg.bool_pot.value =value;
    transmiterFile.vPot[channel].alg.bool_pot.ch2.id =src2;
    transmiterFile.vPot[channel].alg.bool_pot.ch2.type =type2;
}
/**
 * @brief set vpot from pot
 * @param src - pot id
 * @param curve_type - 0: line; 1: smooth curve
 * @param curve - points 0: '--'line; 1: 'V' line
 */
void set_vpot_pot(int channel, int src, int curve_type, int curve)
{
    transmiterFile.vPot[channel].source.id = src;
    transmiterFile.vPot[channel].source.type =SRC_TYPE_POT;
    transmiterFile.vPot[channel].alg.pot_pot.type =curve_type;
    if(curve == 0)
    {
        transmiterFile.vPot[channel].alg.pot_pot.point[0] = -2048;
        transmiterFile.vPot[channel].alg.pot_pot.point[1] = -1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[2] = -1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[3] = 0;
        transmiterFile.vPot[channel].alg.pot_pot.point[4] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[5] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[6] = 2047;
    }
    else if(curve==1)
    {
        transmiterFile.vPot[channel].alg.pot_pot.point[0] = 2047;
        transmiterFile.vPot[channel].alg.pot_pot.point[1] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[2] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[3] = 0;
        transmiterFile.vPot[channel].alg.pot_pot.point[4] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[5] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[6] = 2047;
    }
}
void set_vpot_vpot(int channel, int src, int curve_type, int curve)
{
    transmiterFile.vPot[channel].source.id = src;
    transmiterFile.vPot[channel].source.type =SRC_TYPE_VPOT;
    transmiterFile.vPot[channel].alg.pot_pot.type =curve_type;
    if(curve == 0)
    {
        transmiterFile.vPot[channel].alg.pot_pot.point[0] = -2048;
        transmiterFile.vPot[channel].alg.pot_pot.point[1] = -1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[2] = -1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[3] = 0;
        transmiterFile.vPot[channel].alg.pot_pot.point[4] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[5] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[6] = 2047;
    }
    else if(curve==1)
    {
        transmiterFile.vPot[channel].alg.pot_pot.point[0] = 2047;
        transmiterFile.vPot[channel].alg.pot_pot.point[1] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[2] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[3] = 0;
        transmiterFile.vPot[channel].alg.pot_pot.point[4] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[5] = 1024;
        transmiterFile.vPot[channel].alg.pot_pot.point[6] = 2047;
    }
}

void dirty_file(int type)
{
    if(type ==0)
    {
        transmiterFileDirty =1;
    }else
    {
        modelFileDirty =1;
    }
}

/// get flight phase
void get_flight_phase()
{
}

void taskMix(void *pData)
{
    int t_int;
    int i;
    char t_char;

    //p_rf_driver = &pxx_driver;
    //p_rf_driver->open(p_rf_driver);
    adc_driver.open(&adc_driver);
    //switch_driver.open(&switch_driver);
    led_driver.open(&led_driver);

    while(1)
    {
        /// task pending here
        /// rf_driver.read(&rf_driver, analog_raw, sizeof(analog_raw), 0);

        vTaskDelay(10);

        adc_driver.read(&adc_driver, analog_raw, sizeof(analog_raw), 0);
        switch_driver.read(&switch_driver, switch_sw, sizeof(switch_sw), 0);
        trim_driver.read(&trim_driver, trim_raw, sizeof(trim_raw),0);

        /// calibration
        switch(calibration_fsm)
        {
            case FSM_CALIBRATION_NULL:
                break;
            case FSM_CALIBRATION_START:
                for(i=0; i<NUM_POTS; i++)
                {
                    transmiterFile.cali[i].mid =analog_raw[i];
                    transmiterFile.cali[i].up =transmiterFile.cali[i].mid +400;
                    transmiterFile.cali[i].low =transmiterFile.cali[i].mid -400;
                }
                calibration_fsm =FSM_CALIBRATION_RUN;
                break;
            case FSM_CALIBRATION_RUN:
                for(i=0; i<NUM_POTS; i++)
                {
                    transmiterFile.cali[i].up =transmiterFile.cali[i].up< analog_raw[i]? analog_raw[i]: transmiterFile.cali[i].up;
                    transmiterFile.cali[i].low=transmiterFile.cali[i].low> analog_raw[i]? analog_raw[i]: transmiterFile.cali[i].low;
                }
                break;
            case FSM_CALIBRATION_STOP:
                calibration_fsm = FSM_CALIBRATION_DONE;
                transmiterFileDirty =1;
                break;
            case FSM_CALIBRATION_DONE:
                break;
        }

        calibrate(analog_calibrated, analog_raw, sizeof(analog_raw)/sizeof(analog_raw[0]) );
        input_filter(analog_filtered, analog_calibrated, sizeof(analog_raw)/sizeof(analog_raw[0]) );


        memcpy(analog_pot, analog_filtered, sizeof(analog_pot));
#if 0
        //test curve_7pts
        {
            transmiterFile.vPot[0].alg.pot_pot.point[0] =2048;
            transmiterFile.vPot[0].alg.pot_pot.point[1] =1024;
            transmiterFile.vPot[0].alg.pot_pot.point[2] =1024;
            transmiterFile.vPot[0].alg.pot_pot.point[3] =0;
            transmiterFile.vPot[0].alg.pot_pot.point[4] =1024;
            transmiterFile.vPot[0].alg.pot_pot.point[5] =1024;
            transmiterFile.vPot[0].alg.pot_pot.point[6] =2048;

            volatile int t;
            analog_pot[1]=curve_7pts(analog_pot[0], transmiterFile.vPot[0].alg.pot_pot.point);
        }
        debug_short(analog_pot, -2048, 2048, NUM_POTS);
#endif
        /// trainer port
        trainer_driver.read(&trainer_driver, trainer_raw, sizeof(trainer_raw), 0);
        calibrate_trainer(trainer_calibrated, trainer_raw);

        get_virtual_switches();

#if 0
        /// testing virtual switches
        {
            int i;
            short t_short[NUM_VSW];
            for(i=0; i<NUM_VSW; i++)
            {
                t_short[i] = switch_vsw[i]? 2047: -2048;
            }
            debug_short(t_short, -2048, 2048, NUM_VSW); 
        }
#endif

        get_virtual_pots();
        debug_short(analog_vpot, -2048, 2048, NUM_VPOT);

        get_flight_phase();


    }
}


